from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution, TextSubstitution, EnvironmentVariable, LaunchConfiguration, Command
from launch_ros.substitutions import FindPackageShare
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo
from launch.substitutions import PythonExpression
import platform

def generate_launch_description():
    # --------------------------
    # 1. 系统架构识别
    # --------------------------
    current_arch = platform.machine()
    arch_mapping = {
        "x86_64": {"hikrobot": "amd64", "mindvision": "x64"},
        "aarch64": {"hikrobot": "arm64", "mindvision": "arm64"}
    }
    if current_arch not in arch_mapping:
        raise RuntimeError(f"Unsupported architecture: {current_arch}! Only x86_64/aarch64 are supported.")
    hik_arch_suffix = arch_mapping[current_arch]["hikrobot"]
    mv_arch_suffix = arch_mapping[current_arch]["mindvision"]

    # --------------------------
    # 2. 启动参数声明
    # --------------------------
    declare_params_file = DeclareLaunchArgument(
        'params_file',
        default_value=PathJoinSubstitution([
            FindPackageShare('camera_driver'),
            'config', 
            'camera_config.yaml'
        ]),
        description='相机参数配置文件路径'
    )

    # --------------------------
    # 3. 日志与SDK库路径配置
    # --------------------------
    set_log_format = SetEnvironmentVariable(
        name='RCUTILS_CONSOLE_OUTPUT_FORMAT',
        value='[{time}] [{severity}] [{name}]{message}'
    )
    
    set_log_color = SetEnvironmentVariable(
        name='RCUTILS_COLORIZED_OUTPUT',
        value='1'
    )

    log_arch_info = LogInfo(
        msg=PythonExpression([
            f"'Current architecture: {current_arch}, Hikrobot lib suffix: {hik_arch_suffix}, Mindvision lib suffix: {mv_arch_suffix}'"
        ])
    )

    hikrobot_lib_path = PathJoinSubstitution([
        FindPackageShare('camera_driver'),
        'sdk', 'hikrobot', 'lib', TextSubstitution(text=hik_arch_suffix)
    ])
    mindvision_lib_path = PathJoinSubstitution([
        FindPackageShare('camera_driver'),
        'sdk', 'mindvision', 'lib', TextSubstitution(text=mv_arch_suffix)
    ])

    set_ld_library_path = SetEnvironmentVariable(
        name='LD_LIBRARY_PATH',
        value=[
            hikrobot_lib_path, ':', 
            mindvision_lib_path, ':', 
            EnvironmentVariable('LD_LIBRARY_PATH', default_value='')
        ]
    )

    # --------------------------
    # 4. 相机节点
    # --------------------------
    params_file = LaunchConfiguration('params_file')

    camera_node = Node(
        package='camera_driver',
        # 组件化节点使用 composable_node_container 启动时需用插件形式（如用组件容器请调整为ComposableNode）
        # 这里直接用 Node，后续如需组件容器，请改为 ComposableNodeContainer
        executable='camera_driver_node',
        name='camera_driver_node',
        parameters=[params_file],
        output='screen',
        emulate_tty=True,
        arguments=['--ros-args', '--log-level', 'info']
    )

    # --------------------------
    # 5. 组装Launch描述
    # --------------------------
    return LaunchDescription([
        set_log_format,
        set_log_color,
        log_arch_info,
        set_ld_library_path,
        declare_params_file,  
        camera_node
    ])